#include <cmath>
#include "TetrahedronState.h"
#include "Matrix.h"
#include "Mechanics.h"

/*
Compute elastic reactions using

W(I,J) = (mu/2) (Inv1 - 3 - 2 ln (J)) + lamda /2 (J - 1)^2

This is a compressible Neo-Hookean model.

Inv1 = tr (C)
J = det (F) = sqrt (det (C))

F = Grad (x) = (I + UX^-1)
C = F^T F

This is linear material combined with nonlinear geometry. Note that
W -> infty for J -> 0 (ln J -> -infty if J -> 0) or C -> infty in
any component.


We get:

D_J(W) = (- mu/(2J) +  lamda  (J-1))
D_C(J) = 1/2 J C^{-1}
D_Inv1(W) = mu/2
D_C(Inv1) = Id

D_U(C) = Symmetrify ((Id +UX^-1)^T [.] X^-T)

Hence we have

D_U(W) = (mu/2 Id. + (-mu/2 + lamda (J-1)J/2) C^-1) D_U(C)


To apply the derivative of C, we get

Q : D_U(C) = A : ( F^T (.)X^-1 + ((.) X^-1)^T F)

Since Q:B = tr(Q^T B), we may transform this to

tr [(FQX^-T)^T (.)] + tr [(FQ^TX^-T)^T (.)]

For symmetric Q -- which is the case here, we may write

tr [ (2 FQX^-T)^T (.) ]

hence the derivative is

F * S * X^{-T},

with

S = (mu I + (-mu + lamda (J-1)J ) C^-1)

To obtain the derivative of the force (evaluated in H), we set

DF = H X^{-1}
DC = 2 Symmetrify (F^T DF) 
D(C^-1) = - C^-1 DC C^-1
DS = [  -(lamda (J-1)J -mu ) C^-1 DC  + lamda (2J - 1) J/2 (C^-1 : DC) Id. ] C^-1


Dforce = (DF * S  + F * DSi ) * X^-T
*/

#include "TetrahedronState.h"


Real NeoHookeanElasticEnergy (Real I1, Real I2, Real I3)
{
	return lame_mu *0.5 * (I1 -3 - log (I3))
		+ lame_lambda * 0.5 * Sqr(sqrt(I3) - 1);
}

/*
(setq Tetrahedron_state::neo_hookean_elastic_force

(+
Matrix3::multiply_mm
Matrix3::add_diag

Matrix3::square
Matrix3::determinant
1

Matrix3::invert_to_with_det
Matrix3::scale

Matrix3::diag
Matrix3::add

Matrix3::multiply_mm
Matrix3::multiply_mm  
Matrix3::scale)
)


*/

int NeoHookeanElasticForce (TetrahedronState const*me,
						   Matrix3 * force,
						   Matrix3 const& displacement_mat)
{
	if (me->degenerate_b_)
		return 0;


	Matrix3 C;
	Matrix3 C_inv;
	Matrix3 F;

	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	Real J = F.Determinant ();
	Real detC = Sqr (J);

	Matrix3::InvertToWithDet (C_inv, C, detC);
	Matrix3::Scale (C_inv, (- lame_mu  + lame_lambda *(J-1)*J), C_inv);

	force->Diag (lame_mu);
	Matrix3::Add (*force, *force, C_inv);

	*force =  F * (*force) *me->invloc_transpose_;
	Matrix3::Scale (*force, -2.0 , *force);

	return 277;
}

/*
(setq Tetrahedron_state::neo_hookean_elastic_force_derivative
(+  Matrix3::multiply_mm
Matrix3::add_diag

Matrix3::square
Matrix3::determinant
1

Matrix3::invert_to_with_det
Matrix3::diag
Matrix3::saxpy

Matrix3::multiply_mm
Matrix3::multiply_mtm

Matrix3::double_symmetrify

Matrix3::multiply_mtm
Matrix3::scale
3
Matrix3::inner_product
Matrix3::add_diag 
Matrix3::multiply_mtm
Matrix3::add
Matrix3::multiply_mm
Matrix3::multiply_mm
Matrix3::multiply_mm
Matrix3::scale))

595 flops.
*/
int NeoHookeanElasticForceDerivative (TetrahedronState const*me,
									  Matrix3 *dforce,
									  Matrix3 *force,
									  Matrix3 const &displacement_mat,
									  Matrix3 const &dir_mat)
{
	Matrix3 C;
	Matrix3 C_inv;
	Matrix3 F;


	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	Real J = F.Determinant ();
	Real detC = Sqr( J);

	Matrix3::InvertToWithDet (C_inv, C, detC);

	Matrix3 A;
	A.Diag (lame_mu);

	Matrix3::Saxpy (A,  (- lame_mu  + lame_lambda *(J-1)*J), C_inv, A);

	Matrix3 DF;
	Matrix3::MultiplyMM (DF, dir_mat, me->inverse_location_);

	Matrix3 DC;
	Matrix3::MultiplyMTM (DC, F, DF);

	Matrix3::DoubleSymmetrify (DC, DC);	

	Matrix3 DA = C_inv *DC;
	Matrix3::Scale (DA, - (- lame_mu  + lame_lambda *(J-1)*J), DA);

	DA.AddDiag (lame_lambda * (J/2) * (2*J-1) * Matrix3::InnerProduct (C_inv, DC));

	DA = DA * C_inv;

	*dforce = (DF * A + F * DA) * me->invloc_transpose_;
	Matrix3::Scale (*dforce, -2.0 , *dforce);


	int flops = 595;
	if (force)
	{
		*force = F * A * me->invloc_transpose_;
		Matrix3::Scale (*force, -2.0, *force);

		/*
		(+ Matrix3::scale Matrix3::multiply_mtm  Matrix3::multiply_mtm )
		*/
		flops += 117;
	}

	return flops;
}


#if 0

#define F   (me->precompute_mat_[0])
#define C_inv (me->precompute_mat_[2])
#define A (me->precompute_mat_[3])
#define C_inv_scaled me->precompute_mat_[4]

#define J me->precompute_real_[0]
#define detC me->precompute_real_[1]

/*
(+ Matrix3::multiply_mm
Matrix3::add_diag

Matrix3::square
Matrix3::determinant
1

Matrix3::invert_to_with_det
Matrix3::diag
Matrix3::saxpy)
*/
int NeoHookeanPrecompute (TetrahedronState *me,
						Matrix3 const &displacement_mat)
{
	Matrix3 C;


	Matrix3::MultiplyMM (F, displacement_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	J = F.Determinant ();
	detC = Sqr( J);

	Matrix3::InvertToWithDet (C_inv, C, detC);

	Matrix3 A;
	A.Diag (lame_mu);

	Matrix3::Saxpy (A,  (- lame_mu  + lame_lambda *(J-1)*J), C_inv, A);

	return 160;
}
/*
(+  Matrix3::multiply_mm
Matrix3::multiply_mtm

Matrix3::double_symmetrify

Matrix3::multiply_mtm
Matrix3::scale
3
Matrix3::inner_product
Matrix3::add_diag 
Matrix3::multiply_mtm
Matrix3::add
Matrix3::multiply_mm
Matrix3::multiply_mm
Matrix3::multiply_mm
Matrix3::scale)

*/

int NeoHookeanIncrementalDForce (TetrahedronState const*me,
								Matrix3 *dforce,
								Matrix3 const &dir_mat)
{
	Matrix3 DF;
	Matrix3::MultiplyMM (DF, dir_mat, me->inverse_location_);

	Matrix3 DC;
	Matrix3::MultiplyMTM (DC, F, DF);

	Matrix3::DoubleSymmetrify (DC, DC);	

	Matrix3 DA = C_inv *DC;
	Matrix3::scale (DA, - (- lame_mu  + lame_lambda *(J-1)*J), DA);

	DA.add_diag (lame_lambda * (J/2) * (2*J-1) * Matrix3::InnerProduct (C_inv, DC));

	DA = DA * C_inv;

	*dforce = (DF * A + F * DA) * me->invloc_transpose_;
	Matrix3::Scale (*dforce, -2.0 , *dforce);

	return 435;
}

#endif
